/*
 * pid.c
 *
 *  Created on: 2022年11月18日
 *      Author: paper
 */
#include "pid.h"
//#include "protocol.h"
 ///150000 -> 1m
#define my_abs(num) (((num)>0)?(num):-(num))

PID_IncTypeDef roll_angle_wheel;
PID_IncTypeDef roll_anglespeed_wheel;
PID_IncTypeDef roll_speed_wheel;


PID_IncTypeDef bdc_speed_wheel;
PID_IncTypeDef bdc_speed_wheel1;
PID_IncTypeDef bdc_banlance_wheel;
PID_IncTypeDef bdc_wheel;

PID_IncTypeDef bldc_yaw_anglespeed_wheel;
PID_IncTypeDef bldc_yaw_angle_wheel;
PID_IncTypeDef bldc_yaw_speed_wheel;


float pidout_1f = 0, pidout_2f = 0, pidout_3f = 0, pidout_4f = 0, pidout_5f = 0, pidout_6f = 0;



void PID_Inc_Init(PID_IncTypeDef* sptr, float kp, float ki, float kd) {
    sptr->Ek1 = 0;        //上次偏差值初始化
    sptr->Ek2 = 0;        //上上次偏差值初始化
    sptr->Kp = kp;                //比例常数
    sptr->Ki = ki;                //积分常数
    sptr->Kd = kd;                //微分常数
    sptr->OUT = 0;
}



float Positional_PID(PID_IncTypeDef* PID, float SetValue, float ActualValue, float Max_I) {

    //SetValue期望值，ActualValue实际值
    float PIDInc;
    PID->Ek = SetValue - ActualValue;
    PID->Ek2 += PID->Ek;

    if (PID->Ek2 > Max_I)  PID->Ek2 = Max_I;
    if (PID->Ek2 < -Max_I) PID->Ek2 = -Max_I;

    PIDInc = (PID->Kp * PID->Ek) +
        (PID->Ki * PID->Ek2) +
        (PID->Kd * (PID->Ek - PID->Ek1));
    PID->Ek1 = PID->Ek;
    return PIDInc;
}





/*************************************************************************
 *  函数名称：
 *  功能说明：pid增量式控制器输出
 *  参数说明：
  * @param    pid     pid参数
  * @param    error   pid输入误差
 *  函数返回：PID输出结果   注意输出结果已经包涵了上次结果
 *  修改时间：2020年4月1日
 *  备    注：
 *************************************************************************/

float PidIncCtrl(PID_IncTypeDef *PID, float error)
{
    PID->out_p = PID->Kp * (error - PID->last_error);
    PID->out_i = PID->Ki * error;
    PID->out_d = PID->Kd * ((error - PID->last_error) - PID->last_derivative);

    PID->last_derivative = error - PID->last_error;
    PID->last_error = error;
    PID->out += PID->out_p + PID->out_i + PID->out_d;


    return PID->out;
}




void PID_param_init(void) {
    // 初始化平衡环角度环

        PID_Inc_Init(&roll_speed_wheel, -0.0017f, 0.0001f, 0.0f);////左右平衡速度环  最外环
        PID_Inc_Init(&roll_angle_wheel,7.2f, 0.0f, 30.0f);//左右平衡角度环  中间环
        PID_Inc_Init(&roll_anglespeed_wheel,-85.0f, -1.2f, 0.0f);  //   左右平衡角速度环  最内环

        PID_Inc_Init(&bldc_yaw_speed_wheel, -0.007f, 0.0f, 0.0f);//yaw轴转向速度环   最外环
        PID_Inc_Init(&bldc_yaw_angle_wheel,-2.3f, 0.0f, 20.0f);//yaw轴转向角度环   中间环
        PID_Inc_Init(&bldc_yaw_anglespeed_wheel, -125.0f, 0.0f, 0.0f);//YAW轴平衡角速度环 最内环

        PID_Inc_Init(&bdc_speed_wheel, 0.025f, 0.004f, 0.0f); //前后平衡速度环0.001
        PID_Inc_Init(&bdc_banlance_wheel, -3.2f, 0.0f, -5.0f); //前后平衡角度环
        PID_Inc_Init(&bdc_wheel, 100.0f,1.6f,0.0f);//前后平衡角速度环(60.0f,1.2f,5.0f)

}


